close all
clc

% Defining physical constants
Restlength = 0.8;
D = 1;
m = 0;
g = 9.81;

% Defining Hessian and f
N=7;
h=diag(ones(N,1))+diag(-ones(N-1,1),1);
Matrixone=kron(eye(N),kron(h(1:N-1,:),eye(3)));
Matrixtwo=kron(kron(h(1:N-1,:),eye(N)),eye(3));

H1=[Matrixone; Matrixtwo];
H=H1'*H1;
f=kron(ones(1,N^2),[0 0 m*g]);
Aeq=zeros(4,N^2);
Aeq(1,1)=1; 
Aeq(2,N)=1;
Aeq(3,N^2)=1;
Aeq(4,(N^2-(N-1)))=1;
Aeq=kron(Aeq,eye(3));
beq=[-N/2 -N/2 1 -N/2 N/2 1 N/2 -N/2 1 N/2 N/2 1];
sol = quadprog(H,f,[],[],Aeq,beq,[],[],[])
plotcloth(sol,0.8);